#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "pcl_conversions/pcl_conversions.h"
#include "pcl/point_types.h"
#include "pcl/PCLPointCloud2.h"
#include "pcl/conversions.h"
#include <pcl/io/pcd_io.h>
#include <cmath>
#include <CL/cl.h>
#include "cl_process.hpp"

// 重新定义节点类型
struct PointXYZIntensities
{
    float x;
    float y;
    float z;
    float intensities; 
};

POINT_CLOUD_REGISTER_POINT_STRUCT(
    PointXYZIntensities,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensities, intensities)
);
class PointCloudTransformerNode : public rclcpp::Node
{
public:
    PointCloudTransformerNode() : Node("pointcloud_transformer_node")
    {
        // 声明参数并从YAML文件加载
        this->declare_parameter<double>("rotation.alpha", 0.0);
        this->declare_parameter<double>("rotation.beta", 0.0);
        this->declare_parameter<double>("rotation.omega", 0.0);
        this->declare_parameter<double>("translation.x0", 0.0);
        this->declare_parameter<double>("translation.y0", 0.0);
        this->declare_parameter<double>("translation.z0", 0.0);
        
        // 订阅原始点云
        subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "pointcloud", 10, 
            std::bind(&PointCloudTransformerNode::cloud_callback, this, std::placeholders::_1)
        );
        
        // 发布变换后的点云
        publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("transformed_cloud", 10);
        
        // 初始化OpenCL
        init_opencl();
        
        RCLCPP_INFO(this->get_logger(), "PointCloud Transformer Node initialized");
    }
    
    ~PointCloudTransformerNode()
    {
        // 释放OpenCL资源
        release_opencl();
       }

private:
    void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        // 从参数服务器获取最新参数
        float alpha = this->get_parameter("rotation.alpha").as_double();
        float beta = this->get_parameter("rotation.beta").as_double();
        float omega = this->get_parameter("rotation.omega").as_double();
        float x0 = this->get_parameter("translation.x0").as_double();
        float y0 = this->get_parameter("translation.y0").as_double();
        float z0 = this->get_parameter("translation.z0").as_double();
         RCLCPP_INFO(this->get_logger(),"Received point cloud");
        // 将角度转换为弧度
        alpha = alpha * M_PI / 180.0;
        beta = beta * M_PI / 180.0;
        omega = omega * M_PI / 180.0;
        
        // 计算旋转矩阵 (Z-Y-X 欧拉角)
        float rotation[9];
        calculate_rotation_matrix(alpha, beta, omega, rotation);
        
        // 转换为PCL点云
       // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr laser_cloud(new pcl::PointCloud<pcl::PointXYZI>);
            //自定义点类型解析
          //  pcl::PointCloud<PointXYZIntensities>::Ptr custom_cloud(new pcl::PointCloud<PointXYZIntensities>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr custom_cloud(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::fromROSMsg(*msg, *custom_cloud);
        std::size_t size=custom_cloud->size();
        if (custom_cloud->empty()) {
            RCLCPP_WARN(this->get_logger(), "Received empty point cloud");
            return;
        }
        else RCLCPP_INFO(this->get_logger(),"receive point=%d",size);
        
     
        //  std::string filename = "saved_pointcloud.pcd";
        //  laser_cloud->width=laser_cloud->size();
        //  laser_cloud->height=1;
        //  pcl::io::savePCDFileASCII(filename, *laser_cloud);
        // 使用OpenCL进行点云变换
        RCLCPP_INFO(this->get_logger(),"start gpu!");
        transform_pointcloud(custom_cloud, rotation, x0, y0, z0);
        RCLCPP_INFO(this->get_logger(),"end gpu!=%d",custom_cloud->size());
        custom_cloud->width=custom_cloud->size();
        custom_cloud->height=1;
        // 转换回ROS消息并发布
        sensor_msgs::msg::PointCloud2 output_msg;
        pcl::toROSMsg(*custom_cloud, output_msg);
        output_msg.header = msg->header;  // 保持相同的坐标系和时间戳
        output_msg.header.frame_id  = "map";
        publisher_->publish(output_msg);
       
    }
    

   
   
    
    // ROS相关成员
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
    
   
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PointCloudTransformerNode>());
    rclcpp::shutdown();
    return 0;
}
    